#include "ros/ros.h"
#include "ros/console.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value
#include <math.h>

#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"
#include "move_base_msgs/MoveBaseAction.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseArray.h"

#include <vector>
#include <deque>
#include <set>
#include "tf/transform_listener.h"   
#include "turtlebot_frontier/FSRequest.h"

#include "turtlebot_slam_navigation/get_distances.h"

//Unused
class clearanceRecord {
public:
	geometry_msgs::Pose position;
	double clearance;
	
	clearanceRecord(geometry_msgs::Pose position, double clearance) {
		this->position = position;
		this->clearance = clearance;
	}
	
	clearanceRecord() {
		clearance = 0;
	}
};

//Helper object for drawing on the map, masking, etc...
class point2D {
public:
	int x;
	int y;
	
	bool operator==(const point2D& other) {
		return (other.x == x && other.y == y); 
	}
	
	bool operator!=(const point2D& other) {
		return !(*this == other);
	}
	
	point2D operator+(const point2D& other) {
		point2D output;
		output.x = x+other.x;
		output.y = y+other.y;
		return output;
	}
	
	point2D operator-(const point2D& other) {
		point2D output;
		output.x = x-other.x;
		output.y = y-other.y;
		return output;
	}
	
	point2D operator*(int multiplier) {
		point2D output;
		output.x = x * multiplier;
		output.y = y * multiplier;
		return output;
	}
	
	point2D operator/(int divisor) {
		point2D output;
		output.x = x / divisor;
		output.y = y / divisor;
		return output;
	}
	
	point2D& operator=(const point2D& other) {
		if(this == &other) {
			return *this;
		}
		else {
			x = other.x;
			y = other.y;
		}
		return *this;
	}
	
	point2D& operator+=(const point2D& other) {
		x+=other.x;
		y+=other.y;
		return *this;
	}
	
	point2D& operator-=(const point2D& other) {
		x-=other.x;
		y-=other.y;
		return *this;
	}
	
	point2D& operator*=(int mul) {
		x*=mul;
		y*=mul;
		return *this;
	}
	
	point2D& operator/=(int div) {
		x/=div;
		y/=div;
		return *this;
	}
	
	point2D(int x, int y) {
		this->x = x;
		this->y = y;
	}
	
	point2D() {
		x=0;
		y=0;
	}
	
	point2D left(int delta) {
		point2D tmp = *this;
		tmp.x -= delta;
		return tmp;
	}
	
	point2D right(int delta) {
		point2D tmp = *this;
		tmp.x += delta;
		return tmp;
	}
	
	point2D up(int delta) {
		point2D tmp = *this;
		tmp.y += delta;
		return tmp;
	}
	
	point2D down(int delta) {
		point2D tmp = *this;
		tmp.y -= delta;
		return tmp;
	}
	
	int distanceX(point2D& other) {
		return abs(other.x - x);
	}
	
	int distanceY(point2D& other) {
		return abs(other.y - y);
	}
	
	int distanceM(point2D& other) {
		return distanceX(other) + distanceY(other);
	}
	
	double distance(point2D& other) {
		return sqrt((double)((x-other.x) * (x-other.x)) + (double)((y-other.y) * (y-other.y)));
	}
};

//Helper object for masks
class BitMask {
	
private:
	std::vector<std::vector<bool> > maskData;
	unsigned int _width;
	unsigned int _height;
public:
	void setWidth(int width) {
		_width = width;
		if(maskData.size() > _width) {
			maskData.resize(_width);
		}
		else {
			for(unsigned int i=maskData.size(); i<_width; i++) {
				std::vector<bool> col;
				for(unsigned int j=0; j<_height; j++) {
					col.push_back(false);
				}
				maskData.push_back(col);
			}
		}
	}
	
	void setHeight(int height) {
		//ROS_INFO("Setting height to %d", height);
		_height = height;
		//ROS_INFO("Mask data size: %d", maskData.size());
		if(maskData.size() > 0) {
			for(unsigned int i=0; i<maskData.size(); i++) {
				//ROS_INFO("maskData[%d].size() = %d", i, maskData[i].size());
				if(maskData[i].size() > _height) {
					//ROS_INFO("Size bigger than height, resizing...");
					maskData[i].resize(_height);
				}
				else {
					//ROS_INFO("Size smaller than height, pushing back false values...");
					for(unsigned int j=maskData[i].size(); j<_height; j++) {
						maskData[i].push_back(false);
					}
				}
			}
		}
	}
	
	std::vector<bool>& operator[](unsigned int col) {
		return maskData[col];
	}
	
	void fill() {
		for(unsigned int i=0; i<_width; i++) {
			//Find first and last true values in the column
			
			unsigned int first = _height;
			unsigned int last = _height - 1;
			
			for(unsigned int j=0; j<_height && first == _height; j++) {
				if(maskData[i][j]) first = j;
			} 
			
			if(first != _height) {
				
				for(unsigned int j=_height - 1; j>first && !maskData[i][j]; j--) {
					//std::cout << j << " ";
					last = j;
				}
				
				for(unsigned int j=first; j<last; j++) {
					maskData[i][j] = true;
				}
			}
		}
		
	}

	
	std::vector<char> applyMask(nav_msgs::OccupancyGrid& map, point2D& offset) {
		
		//ROS_INFO("Offset: (%d, %d)", offset.x, offset.y);
		
		//Transform the offset into the map's coordinates
		
		point2D mapOffset = offset;
		mapOffset.x -= (map.info.origin.position.x / map.info.resolution);
		mapOffset.y -= (map.info.origin.position.y / map.info.resolution);
		
		std::vector<char> output;
		for(unsigned int i=0; i<_width; i++) {
			for(unsigned int j=0; j<_height; j++) {
				if(maskData[i][j]) {
					unsigned int xOnMap = i+mapOffset.x;
					unsigned int yOnMap = j+mapOffset.y;
					
					output.push_back(map.data[yOnMap * map.info.width + xOnMap]);
					
					//std::cout << (int)(output.back()) << "\t";
				}
			}
			//std::cout << std::endl;
		}
		
		//std::cout << std::endl;
		
		return output;
	}
	
	void setPixels(std::vector<point2D>& pixelCoordinates) {
		for(unsigned int i=0; i<pixelCoordinates.size(); i++) {
			if(pixelCoordinates[i].x >= 0 && pixelCoordinates[i].x < _width && pixelCoordinates[i].y >= 0 && pixelCoordinates[i].y < _height)
				maskData[pixelCoordinates[i].x][pixelCoordinates[i].y] = true;
		}
		
	}
	
	BitMask(int w, int h) {
		
		_width = 1;
		_height = 1;
		
		std::vector<bool> dummyCol;
		dummyCol.push_back(false);
		maskData.push_back(dummyCol);
		
		this->setWidth(w);
		
		//ROS_INFO("Width set.");
		
		this->setHeight(h);
		
		//ROS_INFO("Height set.");
	}
	
	BitMask() {
		_width = 1;
		_height = 1;
		
		std::vector<bool> dummyCol;
		dummyCol.push_back(false);
		maskData.push_back(dummyCol);
	}
};

class TurtleNav {
	
public:
	// Construst a new TurtleNav object and hook up this ROS node
	// to the simulated robot's velocity control and laser topics
	TurtleNav(ros::NodeHandle& nh) : fsm(FSM_MOVE_FORWARD),rotateStartTime(ros::Time::now()),rotateDuration(0.f),listener(),firstTimePlanning(true),chokePoints(),lastScanMinRange(1) {
		// Initialize random time generator
		srand(time(NULL));
        mapCached = false;
		// Advertise a new publisher for the simulated robot's velocity command topic
		// (the second argument indicates that if multiple command messages are in
		//  the queue to be sent, only the last command will be sent)
		commandPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
		// Subscribe to the simulated robot's laser scan topic and tell ROS to call
		// this->commandCallback() whenever a new message is published on that topic
		laserSub = nh.subscribe("base_scan", 1, &TurtleNav::commandCallback, this);
		//Subscribe to the map
        mapSub = nh.subscribe("map", 1, &TurtleNav::mapCallback, this);
		//Subcribe to the goal
		targetSub = nh.subscribe("goal", 10, &TurtleNav::targetCallback, this);
		//Publish the path to Rviz
		trajectoryPub = nh.advertise<nav_msgs::Path>("trajectory", 10);
		//Create a map client in case the map server answers to a service request
		mapClient = nh.serviceClient<nav_msgs::GetMap>("dynamic_map");
		//Create a client for the frontier scanner
		frontierClient = nh.serviceClient<turtlebot_frontier::FSRequest>("scan_frontier");
		//Ensure that the navigation aids are in the same coordinate system as the map
		chokePoints.header.frame_id = "map"; 
		//Publisher for the navigational aids
		chokePointPub = nh.advertise<geometry_msgs::PoseArray>("chokepoints", 10);
		
		scanForChokePoints();
	};
	
    void mapCallback(const nav_msgs::OccupancyGrid msg) {
        map=msg;
        if(!mapCached) {
            mapCached = true;
            scanFrontiers();
        }
		//If the map is updated, detect the navigational aids.
		scanForChokePoints();
	};
	
	void targetCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
	//If the goal is updated, check the new navigational aids and plan a route
		ROS_INFO("Goal received.");
		goal=*msg;
		scanForChokePoints();
		publishChokePoints();
		replan();
	};
	
	void publishTrajectory() {
		//Convenience function
		ROS_INFO("Trajectory published.");
		trajectoryPub.publish(trajectory);
		publishChokePoints();
	};
	
	void publishChokePoints() {
        //Convenience function
		chokePointPub.publish(chokePoints);
	}
	
	void replan() {
		
		//Recalculate the distances on the chokepoint graph
		recalcChokeMatrix();
		
		ROS_INFO("Replanning...");
		
		ROS_INFO("Map data:");
		std::cout << "Width: " << map.info.width << std::endl;
		std::cout << "Height: " << map.info.height << std::endl;
		std::cout << "Resolution: " << map.info.resolution << std::endl;
		std::cout << "Frame ID: " << map.header.frame_id << std::endl;
		std::cout << "Origin: (" << map.info.origin.position.x << ", " << map.info.origin.position.y << ")" << std::endl;
		
		//Get the position of the robot on the map
		
		geometry_msgs::PoseStamped dummy;
		
		dummy.header.stamp = ros::Time::now();
		dummy.header.frame_id = "base_link";
		dummy.pose.position.x = dummy.pose.position.y = dummy.pose.position.z = 0;
		dummy.pose.orientation.x = dummy.pose.orientation.y = dummy.pose.orientation.z = 0;
		dummy.pose.orientation.w = 1;
		
		geometry_msgs::PoseStamped dummyPose;
		
		tf::StampedTransform robotTransform;
		
		try {
			listener.transformPose("map", dummy, dummyPose);
		}       
		catch(const tf::TransformException & ex) {
			ROS_ERROR("TF error: %s", ex.what());
		}                
		
		
		//Plan the path from the current position to the goal
		if(canGo(dummyPose.pose.position, goal.pose.position)) {
			//If it can get to the destination in a straight line,
			//it should.
			trajectory.poses.resize(2);
			trajectory.poses[0] = dummyPose;
			trajectory.poses[1] = goal;
		}
		else {
			trajectory = path(dummyPose.pose.position, goal.pose.position);
		}
		
		trajectory.header.stamp = ros::Time::now();
		trajectory.header.frame_id = "map";
		
		//Publish the path
		publishTrajectory();
	};

    bool getDistances(turtlebot_slam_navigation::get_distances::Request& req, turtlebot_slam_navigation::get_distances::Response& res) {

        //Recalculate the distances on the chokepoint graph
        recalcChokeMatrix();

        ROS_INFO("Getting distances...");

        ROS_INFO("Map data:");
        std::cout << "Width: " << map.info.width << std::endl;
        std::cout << "Height: " << map.info.height << std::endl;
        std::cout << "Resolution: " << map.info.resolution << std::endl;
        std::cout << "Frame ID: " << map.header.frame_id << std::endl;
        std::cout << "Origin: (" << map.info.origin.position.x << ", " << map.info.origin.position.y << ")" << std::endl;

        //Get the position of the robot on the map

        geometry_msgs::PoseStamped dummy;

        dummy.header.stamp = ros::Time::now();
        dummy.header.frame_id = "base_link";
        dummy.pose.position.x = dummy.pose.position.y = dummy.pose.position.z = 0;
        dummy.pose.orientation.x = dummy.pose.orientation.y = dummy.pose.orientation.z = 0;
        dummy.pose.orientation.w = 1;

        geometry_msgs::PoseStamped dummyPose;

        tf::StampedTransform robotTransform;

        try {
            listener.transformPose("map", dummy, dummyPose);
        }
        catch(const tf::TransformException & ex) {
            ROS_ERROR("TF error: %s", ex.what());
        }

        //Set up the goal conditions & the output

        res.distances.resize(req.poses.poses.size());
        std::vector<bool> pathToBeFound;
        for(int i=0; i<req.poses.poses.size(); i++) {
            pathToBeFound.push_back(true);
            res.distances[i].pose = req.poses.poses[i];
            res.distances[i].distance = INFINITY;
        }

        //Create the spliced matrix, now with every requested pose as goal.

        std::vector<std::vector<double> > splicedMatrix;

        int n = req.poses.poses.size();

        std::cout << "path: started" << std::endl;
        //Get a copy of the choke point matrix with n+1 (n=amount of points) extra rows & cols
        for(unsigned int i=0; i<chokeMatrix.size()+n+1; i++) {


            std::vector<double> row;
            for(unsigned int j=0; j<chokeMatrix.size()+n+1; j++) {
                if(i<chokeMatrix.size() && j<chokeMatrix.size()) {
                    row.push_back(chokeMatrix[i][j]);
                }
                else {
                    row.push_back(0);
                }
            }
            splicedMatrix.push_back(row);
        }

        //Calculate the distances

        for(unsigned int i=0; i<=chokePoints.poses.size(); i++) {
            for(unsigned int j=chokePoints.poses.size()+1; j<splicedMatrix.size(); j++) {
                if(i == j) splicedMatrix[i][j] = 0;
                else {
                    if(i > splicedMatrix.size() - n - 2 || j > splicedMatrix.size() - n - 2) {
                        //Only calculate the start and end points,
                        //the rest are already in the chokeMatrix.
                        geometry_msgs::Point p_i;
                        geometry_msgs::Point p_j;
                        double bias_i = 1.0;
                        double bias_j = 1.0;
                        if(i < chokePoints.poses.size()) {
                            p_i = chokePoints.poses[i].position;
                            //Bias towards nav points
                            if(tf::getYaw(chokePoints.poses[i].orientation) == 0) bias_i = 1.5;
                        }
                        else {
                            if(i == chokePoints.poses.size()) {
                                p_i = dummy.pose.position;
                            }
                            //else {
                            //    p_i = req.poses.poses[i-chokePoints.poses.size()-1].position;
                            //}
                        }
                        if(j < chokePoints.poses.size()) {
                            p_j = chokePoints.poses[j].position;
                            if(tf::getYaw(chokePoints.poses[j].orientation) == 0) bias_j = 1.5;
                        }
                        else {
                            if(j == chokePoints.poses.size()) {
                                p_j = dummy.pose.position;
                            }
                            else {
                                p_j = req.poses.poses[j-chokePoints.poses.size()-1].position;;
                            }
                        }
                        if(splicedMatrix[i][j] == 0 && i<=chokePoints.poses.size()) {
                            if(canGo(p_i, p_j)) {
                                //canGo is asymmetric
                                splicedMatrix[i][j] = distance(p_i, p_j) * bias_i * bias_j;
                                if(i == chokePoints.poses.size()) {
                                    pathToBeFound[j-chokePoints.poses.size()-1] = false;
                                    res.distances[j-chokePoints.poses.size()-1].distance = splicedMatrix[i][j];
                                }
                            }
                            else {
                                splicedMatrix[i][j] = 0;
                                pathToBeFound[j-chokePoints.poses.size()-1] = false;
                            }
                        }
                    }
                }

            }
        }
        ROS_DEBUG("Spliced matrix done.");

        //A straight-up implementation of Dijsktra search on the matrix

        //Setup heuristics (as it's derived from an implementation of A*, I'll just set the heuristics
        //to 0.)

        std::vector<double> heur;
        for(unsigned int j=0; j<splicedMatrix.back().size(); j++) {
            heur.push_back(0);
        }

        std::vector<bool> closed;
        std::vector<unsigned int> open;
        std::vector<int> came_from;
        nav_msgs::Path bestPath;
        std::vector<double> g_scores;
        std::vector<double> f_scores;

        for(unsigned int j=0; j<splicedMatrix.size(); j++) {
            closed.push_back(false);
            g_scores.push_back(DBL_MAX);
            f_scores.push_back(DBL_MAX);
            came_from.push_back(-1);
        }

        open.push_back(splicedMatrix.size()-n-1);
        g_scores[splicedMatrix.size()-n-1] = 0;
        f_scores[splicedMatrix.size()-n-1] = 0;

        bool goalsLeft = true; //This variable tracks if there are goals left.

        while(open.size() > 0 && goalsLeft) {

            unsigned int current = open[0];
            int currentIndex = 0;
            for(unsigned int i=0; i<open.size(); i++) {
                if(f_scores[open[i]] < f_scores[current]) {
                    current = open[i];
                    currentIndex = i;
                }
            }

            if(current >= splicedMatrix.size()-n) {

                pathToBeFound[current - chokePoints.poses.size() - 1] = false;
                res.distances[current - chokePoints.poses.size() - 1].distance = f_scores[current];
            }

            open.erase(open.begin()+currentIndex);
            closed[current] = true;

            for(unsigned int i=0; i<splicedMatrix.size(); i++) {
                if(splicedMatrix.at(current).at(i) != 0) {
                    if(!closed[i]) {
                        double tmp_g = g_scores[current] + splicedMatrix[current][i];
                        bool is_in_open = false;
                        for(unsigned int j=0; j<open.size() && !is_in_open; j++) {
                            is_in_open = (open[j] == i);
                        }

                        if(!is_in_open || tmp_g < g_scores[i]) {
                            came_from[i] = current;
                            g_scores[i] = tmp_g;
                            f_scores[i] = tmp_g + heur[i];
                            if(!is_in_open) {
                                open.push_back(i);
                            }
                        }
                    }
                }
            }

            goalsLeft = false;
            for(int i=0; i<pathToBeFound.size(); i++) {
                goalsLeft = (goalsLeft || pathToBeFound[i]);
            }

        }
        return true;
    };
	
	void scanForChokePoints() {
		//Scan the whole map for choke points.
		
		//First, threshold the map and convert it into a more usable format
		
		ROS_INFO("scanForChokePoints started");
		
		chokePoints.poses.resize(0);
		
		if(map.info.width > 0 && map.info.height > 0) {
			
			std::vector<std::vector<int> > wavefrontMap;
			std::vector<std::vector<bool> > isFrontMap;
			std::vector<std::vector<bool> > nextFrontMap;
			
			
			//Initialize the matrices
			for(unsigned int x=0; x<map.info.width; x++) {
				std::vector<int> wfcol;
				std::vector<bool> ifcol;
				std::vector<bool> nfcol;
				for(unsigned int y=0; y<map.info.height; y++) {
					wfcol.push_back(INT_MAX);
					ifcol.push_back(false);
					nfcol.push_back(false);
				}
				wavefrontMap.push_back(wfcol);
				isFrontMap.push_back(ifcol);
				nextFrontMap.push_back(nfcol);
			}
			
			unsigned int minX = map.info.width - 1;
			unsigned int minY = map.info.height - 1;
			unsigned int maxX = 0;
			unsigned int maxY = 0;
			
			
			//Convert the map into the wavefront map.
			for(unsigned int x=1; x< map.info.width-1; x++) {
				for(unsigned int y=1; y< map.info.height-1; y++) {
					
					char mapCell = map.data[y * map.info.width + x];
					if(mapCell < 0) {
						wavefrontMap[x][y] = -1;
					}
					else {
						if(mapCell < 25) {
							wavefrontMap[x][y] = INT_MAX;
						}
						else {
							wavefrontMap[x][y] = 0;
							isFrontMap[x][y] = true;
						}
						if(x < minX) minX = x;
						if(y < minY) minY = y;
						if(x > maxX) maxX = x;
						if(y > maxY) maxY = y;
					}
				}
			}
			
			unsigned long int changed = 0;
			
			//Project the waves from the walls
			do{
				changed = 0;
				for(unsigned int x = minX; x<maxX; x++) {
					for(unsigned int y=minY; y<maxY; y++) {
						for(unsigned int a = x-1; a <= x+1; a++) {
							for(unsigned int b=y-1; b<=y+1; b++) {
								if(isFrontMap[a][b]) {
									if(wavefrontMap[x][y] > wavefrontMap[a][b]+1) {
										wavefrontMap[x][y] = wavefrontMap[a][b]+1;
										changed++;
										nextFrontMap[x][y] = true;
									}
									nextFrontMap[a][b] = false;
								}
							}
						}
					}
				}
				for(unsigned int x=minX; x<maxX; x++) {
					for(unsigned int y=minY; y<maxY; y++) {
						isFrontMap[x][y] = nextFrontMap[x][y];
						nextFrontMap[x][y] = false;
					}
				}
				
			} while(changed > 0);
			
			//ROS_INFO("scanForChokePoints: waves projected.");
			
			// Mark the ridges. A point is considered a ridge if it's a local maximum on either dimension or on the diagonals.
			
			std::vector<std::vector<bool> >ridgeMap;
			
			for(unsigned int x=0; x<map.info.width; x++) {
				std::vector<bool> rcol;
				for(unsigned int y=0; y<map.info.height; y++) {
					rcol.push_back(false);
				}
				ridgeMap.push_back(rcol);
			}
			
			int ridgeCount = 0;
			
			for(unsigned int x=minX; x<maxX; x++) {
				for(unsigned int y=minY; y<maxY; y++) {
					if((wavefrontMap[x][y] > wavefrontMap[x+1][y] && wavefrontMap[x][y] > wavefrontMap[x-1][y])) {
						ridgeMap[x][y] = true;
					}
					else {
						if((wavefrontMap[x][y] > wavefrontMap[x][y+1] && wavefrontMap[x][y] > wavefrontMap[x][y-1])) {
							ridgeMap[x][y] = true;
						}
						else { 
							if((wavefrontMap[x][y] > wavefrontMap[x+1][y+1] && wavefrontMap[x][y] > wavefrontMap[x-1][y-1])) {
								ridgeMap[x][y] = true;
							}
							else {
								if((wavefrontMap[x][y] > wavefrontMap[x+1][y-1] && wavefrontMap[x][y] > wavefrontMap[x-1][y+1])) {
									ridgeMap[x][y] = true;
								}
								else {
									ridgeMap[x][y] = false;
								}
							}
						}
						
					}
					if(ridgeMap[x][y]) ridgeCount++;
				}
			}
			
			// Find local minimums on ridges, mark them as choke points
			// Also find local maximums on ridges, mark them as navpoints
			// Both sets will be used to help navigation.
			
			int foundChokePoints = 0;
			int foundNavPoints = 0;
			
			point2D lastChokePoint(INT_MAX, INT_MAX);
			point2D lastNavPoint(INT_MAX, INT_MAX);
			
			for(unsigned int x=minX; x<maxX; x++) {
				for(unsigned int y=minY; y<maxY; y++) {
					if(ridgeMap[x][y]) {
						bool isLocMin = true;
						bool isLocMax = true;
						for(unsigned int a=x-1; a<=x+1 && isLocMin; a++) {
							for(unsigned int b=y-1; b<=y+1 && isLocMin; b++) {
								if(ridgeMap[a][b]) {
									if(wavefrontMap[a][b] < wavefrontMap[x][y]) {
										isLocMin = false;
									}
								}
							}
						}
						for(unsigned int a=x-1; a<=x+1 && isLocMax; a++) {
							for(unsigned int b=y-1; b<=y+1 && isLocMax; b++) {
								if(ridgeMap[a][b]) {
									if(wavefrontMap[a][b] > wavefrontMap[x][y]) {
										isLocMax = false;
									}
								}
							}
						}
						if(isLocMin && wavefrontMap[x][y] > BOT_RADIUS / map.info.resolution) {
							point2D cp2d(x, y);
							lastChokePoint = cp2d;
							double chokeX = map.info.resolution * (x + 0.5) + map.info.origin.position.x;
							double chokeY = map.info.resolution * (y + 0.5) + map.info.origin.position.y;
							
							geometry_msgs::Pose cp;
							cp.position.x = chokeX;
							cp.position.y = chokeY;
							cp.position.z = 0;
							cp.orientation = tf::createQuaternionMsgFromYaw(0);
							
							addChokePoint(cp, wavefrontMap[x][y]);
							
							foundChokePoints++;
						}
						
						if(isLocMax && wavefrontMap[x][y] > BOT_RADIUS / map.info.resolution) {
							point2D np2d(x, y);
							lastChokePoint = np2d;
							double navX = map.info.resolution * (x + 0.5) + map.info.origin.position.x;
							double navY = map.info.resolution * (y + 0.5) + map.info.origin.position.y;
							
							geometry_msgs::Pose np;
							np.position.x = navX;
							np.position.y = navY;
							np.position.z = 0;
							np.orientation = tf::createQuaternionMsgFromYaw(-1*M_PI);
							
							addNavPoint(np, wavefrontMap[x][y]);
							
							foundNavPoints++;
							//}
						}
					}
				}
			}	
			
			publishChokePoints();
			
			ROS_INFO("scanForChokePoints: %d choke points and %d nav points found", foundChokePoints, foundNavPoints);
		}
	};
	
	// Send a velocity command 
	void move(double linearVelMPS, double angularVelRadPS) {
		geometry_msgs::Twist msg; // The default constructor will set all commands to 0
		msg.linear.x = linearVelMPS;
		msg.angular.z = angularVelRadPS;
		commandPub.publish(msg);
	};       
	
	//Main navigation function. Follows the given trajectory.
	void followTrajectory(float tolerance_position, float tolerance_orientation) {
        fsm = FSM_ROTATE;
		if(trajectory.poses.size() > 0) {   
			tf::StampedTransform robotRelativeToTrajectory;      
			geometry_msgs::TransformStamped roboTransform;
			try {
				listener.lookupTransform("map", "base_link", ros::Time::now(), robotRelativeToTrajectory);
				tf::transformStampedTFToMsg(robotRelativeToTrajectory, roboTransform);
				float distanceFromNextPoint = distance(roboTransform.transform.translation, trajectory.poses[0].pose.position);

				//Check whether the robot has arrived to a destination
				if(distanceFromNextPoint < tolerance_position) {
					
                    publishTrajectory();
					
					//Remove the first point from the trajectory by shifting the points to the left
					for(unsigned int i=0; i<trajectory.poses.size()-1; i++) {
						trajectory.poses[i] = trajectory.poses[i+1];
					}
					//Then resize the poses
                    trajectory.poses.resize(trajectory.poses.size()-1);
					
					//If the last point was cleared, publish the empty trajectory
                    if(trajectory.poses.size() == 0) {
                        publishTrajectory();
                    }
				}
				else {
					
					std::cout << "Outside tolerance. Checking orientation." << std::endl;
					//If the robot is not in the desired range,
					//check if it's facing the direction of the next waypoint
					float dx = trajectory.poses[0].pose.position.x - roboTransform.transform.translation.x;
					float dy = trajectory.poses[0].pose.position.y - roboTransform.transform.translation.y;
					float desiredYaw = atan2(dy, dx) * 360.0 / (2*M_PI);
					float actualYaw = tf::getYaw(roboTransform.transform.rotation) * 360.0 / (2*M_PI);
					
					float yawDiff = actualYaw - desiredYaw;
					float aYawDiff = fabs(yawDiff);
					while(aYawDiff >= 180.0) {
						if(yawDiff < 0) yawDiff += 360.0;
						else yawDiff -= 360.0;
						aYawDiff = fabs(yawDiff);
					}
					//If the actual yaw is in the tolerated range, start moving.
					if(aYawDiff < tolerance_orientation * 360.0 / (2*M_PI)) {      
						fsm = FSM_MOVE_FORWARD;
						std::cout << yawDiff << " is within tolerance (" << tolerance_orientation * 360.0 / (2*M_PI) << "°). Moving forward." << std::endl;
						currentSpeed = speedForDistance(distanceFromNextPoint); 
						move(currentSpeed, -0.1*((actualYaw-desiredYaw)/360.0)*2*M_PI);
						
						
						//Change the range in which obstacles are detected.
						//Speed and sensor range affect each other.
						//In the vicinity of obstacles, the robot moves slower,
						//in free space it moves faster.
						//Conversely, if the robot is moving fast, objects are detected
						//further away.
						if(currentSpeed < 1) currentSensorRange = PROXIMITY_RANGE_M * currentSpeed * currentSpeed;
						else currentSensorRange = PROXIMITY_RANGE_M * sqrt(currentSpeed);
						
						if(currentSensorRange < MIN_OBSTACLE_RANGE) currentSensorRange = MIN_OBSTACLE_RANGE;
						if(currentSensorRange > MAX_OBSTACLE_RANGE) currentSensorRange = MAX_OBSTACLE_RANGE;
					}
					else {
						fsm = FSM_ROTATE;
						std::cout << yawDiff << " is outside tolerance. Rotating." << std::endl; 
						//Otherwise start turning. The larger the difference, the faster the rotation.
						std::cout << "Rotate speed: " << -1*rotSpeedForDifference(yawDiff) << " rad / s" << std::endl;
						move(0, -1*rotSpeedForDifference(yawDiff));
					}
				}
			}
			catch (const tf::TransformException & ex) {
				ROS_ERROR("TF error in followTrajectory: %s", ex.what());
			}
		}
		else {
            if(currentSpeed > 0) {
                ROS_INFO("Arrived at the destination.------------------");
                ROS_INFO("Initiating frontier scan...");
                scanFrontiers();
            }
			currentSensorRange = MIN_OBSTACLE_RANGE;
			currentSpeed = 0;
			move(0, M_PI/8.0);
			//If it's at the destination, start turning slowly to scan the area.
		}
	}               
	
	double distance(geometry_msgs::Point a, geometry_msgs::Point b) { //Euler-distance
		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y) + (a.z-b.z)*(a.z-b.z));
	}
	//Someone @WG forgot that Vector3 and Point are identical...
	double distance(geometry_msgs::Point a, geometry_msgs::Vector3 b) {
		geometry_msgs::Point p;
		p.x = b.x;
		p.y = b.y;
		p.z = b.z;
		return distance(a, p);
	}
	double distance(geometry_msgs::Vector3 a, geometry_msgs::Point b) {
		geometry_msgs::Point p;
		p.x = a.x;
		p.y = a.y;
		p.z = a.z;
		return distance(p, b);
	}
	double distance(geometry_msgs::Vector3 a, geometry_msgs::Vector3 b) {
		geometry_msgs::Point p;
		p.x = b.x;
		p.y = b.y;
		p.z = b.z;
		return distance(a, p);
	}
	
	// Process the incoming laser scan message
	void commandCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
		//Request the updated map
        if(map.data.size() == 0 || ros::Time::now().sec - map.header.stamp.sec >= 10) {
            nav_msgs::GetMap srv_map;
            if(mapClient.call(srv_map)) {
			//ROS_INFO("Map service called successfully");
                map = srv_map.response.map;
                //replan();
            }
            else {
                ROS_ERROR("Map service call failed.");
            }
        }

		//Find the minimal range in the scanned area to calculate the speed of the robot
		unsigned int minIndex = ceil((MIN_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		unsigned int maxIndex = ceil((MAX_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		float closestRange = msg->ranges[minIndex];
		for (unsigned int currIndex = minIndex + 1; currIndex < maxIndex; currIndex++) {
			if (msg->ranges[currIndex] < closestRange) {
				closestRange = msg->ranges[currIndex];
			}
        }

        int obstacleIndex = -1;
		
		lastScanMinRange = closestRange;
		
		//Check if there's an obstacle, try turning away from the obstacles and replan route if there is one.
        if(isObstacle(currentSensorRange, msg, obstacleIndex) && fsm == FSM_MOVE_FORWARD) {
			if(obstacleIndex > (minIndex + (maxIndex - minIndex)/2.0)) {
				move(0, -M_PI/12.0);
			}
			else {
				move(0, M_PI/12.0);
			}
			if(trajectory.poses.size() > 0) {
				ROS_WARN("Obstacle; replan.");
				trajectory.poses.resize(0);
				publishTrajectory();
				replan();
			}
		}
		
	};   

    bool isObstacle(double forwardRange, const sensor_msgs::LaserScan::ConstPtr& scanMsg, int& obstacleIndex) {

        //Geometric calculations; essentially scans if there is anything in a 0.5*forwardRange box starting from the robot

        obstacleIndex = -1;

        unsigned int minIndex = ceil((-M_PI/2.0 - scanMsg->angle_min) / scanMsg->angle_increment);
        unsigned int maxIndex = ceil((M_PI/2.0 - scanMsg->angle_min) / scanMsg->angle_increment);
        double angleLimitMin = atan2(forwardRange, 0.25);
        double angleLimitMax = atan2(forwardRange, -0.25);
        for (unsigned int currIndex = minIndex; currIndex <= maxIndex; currIndex++) {
            double angle = currIndex * scanMsg->angle_increment + scanMsg->angle_min + M_PI/2.0;
            if(angle < angleLimitMin || angle > angleLimitMax) {
                double cosine = cos(angle);
                if(fabs(cosine * scanMsg->ranges[currIndex]) < 0.25) {
                    obstacleIndex = currIndex;
                    return true;
                }
            }
            else {
                double sine = sin(angle);
                if(sine * scanMsg->ranges[currIndex] < forwardRange) {
                    obstacleIndex = currIndex;
                    return true;
                }

            }
        }

        return false;
    }
	
	
	
	// Main FSM loop for ensuring that ROS messages are
	// processed in a timely manner, and also for sending
	// velocity controls to the simulated robot based on the FSM state
	void spin() {
		
		scanForChokePoints();
		
		static int cycleCounter = 0;
		ros::Rate rate(10); // Specify the FSM loop rate in Hz
		while (ros::ok()) { // Keep spinning loop until user presses Ctrl+C
			
			followTrajectory(0.1, 0.05);
			
			//Scan the map for choke points every 5 seconds
			if((cycleCounter % 50) == 0) {
				scanForChokePoints();
			}
			
			//Initialize a frontier detection every 30 seconds
            if((cycleCounter % 300) == 0) {
				cycleCounter = 0;
				scanFrontiers();
			}
			
			cycleCounter++;
			ros::spinOnce();
			rate.sleep();
		}
	};
	
	//Convenience function for initiating a frontier scan
	void scanFrontiers() {
		ROS_INFO("Frontier detection...");
		turtlebot_frontier::FSRequest request;
		tf::StampedTransform robotRelativeToTrajectory;      
		geometry_msgs::TransformStamped roboTransform;
		try {
            listener.lookupTransform("map", "base_link", ros::Time::now(), robotRelativeToTrajectory);
            tf::transformStampedTFToMsg(robotRelativeToTrajectory, roboTransform);
			request.request.robotPose.position.x = roboTransform.transform.translation.x;
			request.request.robotPose.position.y = roboTransform.transform.translation.y;
			request.request.robotPose.position.z = roboTransform.transform.translation.z;
			request.request.robotPose.orientation = roboTransform.transform.rotation;
			frontierClient.call(request);
		}
		catch (const tf::TransformException & ex) {
			ROS_ERROR("TF error in followTrajectory: %s", ex.what());
		}
	}
	
	
	double speedForDistance(double distance) {
		
		//Calculate the distance based on the closest obstacle
		
		if(distance > 1.5) return 1.0 * sqrt(lastScanMinRange);
		else {
			if(distance < 0.3) return 0.3 * sqrt(lastScanMinRange);
			else return distance * sqrt(lastScanMinRange);
		}
	}
	
		//Calculate the rotation speed for a given angle difference (in degrees)
	double rotSpeedForDifference(double degDifference) {
		
		double radDifference = fabs((degDifference / 360.0) * 2.0 * M_PI);
		
		double direction = (degDifference < 0)?-1.0:1.0;
		
		if(radDifference < 1.0) return (double)direction * (double)sqrt(radDifference);
		
		else {
			if(radDifference > sqrt(2.0*M_PI)) return direction * 2.0 * M_PI;
			else return direction * radDifference * radDifference;
		}
	}
	
	void smoothClearances() {
		//Unused
		
		//std::cout << "Smooth clearances" << std::endl;
		smoothed.clear();
		for(unsigned int i=0; i<clearances.size(); i++) {
			smoothed.push_back(clearances[i]);
		}
		if(clearances.size() > 1) {
			smoothed[0].clearance = 0.8*clearances[0].clearance + 0.2 * clearances[1].clearance;
			smoothed[smoothed.size()-1].clearance = 0.8*clearances[smoothed.size()-1].clearance + 0.2 * clearances[smoothed.size()-2].clearance;
		}
		for(unsigned int i=1; i<clearances.size()-1; i++) {
			smoothed[i].clearance = 0.15 * clearances[i-1].clearance * 0.15 * clearances[i+1].clearance + 0.7*clearances[i].clearance;
		}
		//std::cout << "Smooth clearances ended." << std::endl;
	}
	
	clearanceRecord * localMinimum() {
		//Unused
		
		clearanceRecord * lMin = NULL;
		if(smoothed.size() > 2) {
			
			//Find the local minimum if it exists. It can't be the first or the last.
			int minIndex = (smoothed.front().clearance < smoothed.back().clearance) ? 0 : smoothed.size()-1;
			double minValue = smoothed[minIndex].clearance;
			minIndex = -1;
			for(unsigned int i=1; i<smoothed.size()-1; i++) {
				if(smoothed[i].clearance < minValue) {
					minIndex = i;
					minValue = smoothed[i].clearance;
				}
			}
			if(minIndex != -1) {
				double d1 = fabs(smoothed[minIndex].clearance - smoothed[minIndex-1].clearance);
				double d2 = fabs(smoothed[minIndex].clearance - smoothed[minIndex+1].clearance);
				if(d1 > CLEARANCE_MIN_DIFF && d2 > CLEARANCE_MIN_DIFF) {
					lMin = &(smoothed[minIndex]);
				}
			}
		}
		
		return lMin;
		
	}
	
	void addChokePoint(geometry_msgs::Pose chokepoint) {
		addChokePoint(chokepoint, INT_MAX);
	}
	
	void addChokePoint(geometry_msgs::Pose chokepoint, int wavefrontValue) {
		//std::cout << "Add choke point" << std::endl;
		bool dupe = false;
		double waveClrValue = map.info.resolution * wavefrontValue;
		waveClrValue -= MIN_OBSTACLE_RANGE;
		if(waveClrValue < 2*map.info.resolution) waveClrValue = map.info.resolution * map.info.resolution;
		waveClrValue = sqrt(waveClrValue);
		if(waveClrValue > 2) waveClrValue = 2;
		
		for(unsigned int i=0; i<chokePoints.poses.size() && !dupe; i++) {
			if(distance(chokepoint.position, chokePoints.poses[i].position) < waveClrValue * CHOKEPOINT_MIN_DISTANCE) {
				dupe = true;
			}
		}
		
		if(!dupe) {
			//ROS_INFO("Choke point added at (%f, %f)", chokepoint.position.x, chokepoint.position.y);
			chokePoints.poses.resize(chokePoints.poses.size()+1);
			chokePoints.poses[chokePoints.poses.size()-1] = chokepoint;
			publishChokePoints();
			//ROS_INFO("Resizing choke matrix...");
			
			//Add a new row to the matrix
			chokeMatrix.resize(chokePoints.poses.size());
			
			//Add a new column as well
			for(unsigned int i=0; i<chokeMatrix.size()-1; i++) {
				chokeMatrix[i].push_back(0);
			}
			
			//Then fill up the recently added row
			for(unsigned int i=0; i<chokePoints.poses.size(); i++) {
				chokeMatrix.back().push_back(0);
			}
		}
	}
	
	void addNavPoint(geometry_msgs::Pose navpoint) {
		addNavPoint(navpoint, INT_MAX);
	}
	
	void addNavPoint(geometry_msgs::Pose navpoint, int wavefrontValue) {
		//std::cout << "Add nav point" << std::endl;
		bool dupe = false;
		
		//The function is identical to addChokePoint.
		//It's a separate function because nav points might be handled
		//differently later (given preference in path planning, etc...)
		
		double waveClrValue = map.info.resolution * wavefrontValue;
		waveClrValue -= BOT_RADIUS;
		if(waveClrValue < 2*map.info.resolution) waveClrValue = map.info.resolution * map.info.resolution;
		waveClrValue = sqrt(waveClrValue);
		if(waveClrValue > 2) waveClrValue = 2;
		
		for(unsigned int i=0; i<chokePoints.poses.size() && !dupe; i++) {
			if(distance(navpoint.position, chokePoints.poses[i].position) < CHOKEPOINT_MIN_DISTANCE * waveClrValue) {
				dupe = true;
			}
		}
		
		if(!dupe) {
			//ROS_INFO("Nav point added at (%f, %f)", navpoint.position.x, navpoint.position.y);
			chokePoints.poses.resize(chokePoints.poses.size()+1);
			chokePoints.poses[chokePoints.poses.size()-1] = navpoint;
			publishChokePoints();
			//ROS_INFO("Resizing choke matrix...");
			
			//Add a new row
			chokeMatrix.resize(chokePoints.poses.size());
			
			//Add a new column
			for(unsigned int i=0; i<chokeMatrix.size()-1; i++) {
				chokeMatrix[i].push_back(0);
			}
			
			//Fill up the added row
			for(unsigned int i=0; i<chokePoints.poses.size(); i++) {
				chokeMatrix.back().push_back(0);
			}
		}
	}
	
	void recalcChokeMatrix() {
		
		std::cout << "Recalculating choke point matrix." << std::endl;
		
		
		for(unsigned int i=0; i<chokeMatrix.size(); i++) {
			for(unsigned int j=0; j<chokeMatrix.size(); j++) {
				if(i == j) chokeMatrix[i][j] = 0;
				else {
					if(chokeMatrix[i][j] == 0) { 
						//The method is always launched from an empty choke matrix
						//so it can be allowed to be optimistic
						std::vector<std::vector<point2D> > * lines = canGo(chokePoints.poses[i].position, chokePoints.poses[j].position); 
						if(lines != NULL) {
							//canGo is asymmetric, so chokeMatrix[i][j] isn't necessary equal to chokeMatrix[j][i]
							chokeMatrix[i][j] = distance(chokePoints.poses[i].position, chokePoints.poses[j].position);
							//Bias towards nav points
							if(tf::getYaw(chokePoints.poses[i].orientation) == 0) chokeMatrix[i][j] *= 1.5;
							if(tf::getYaw(chokePoints.poses[j].orientation) == 0) chokeMatrix[i][j] *= 1.5;
						}
						else {
							chokeMatrix[i][j] = 0;
						}
					}
				}
				
			}
		}
	}
	
	nav_msgs::Path path(geometry_msgs::Point from, geometry_msgs::Point to) {
		std::vector<std::vector<double> > splicedMatrix;
		
		std::cout << "path: started" << std::endl;
		//Get a copy of the choke point matrix with two extra rows & cols
		for(unsigned int i=0; i<chokeMatrix.size()+2; i++) {
			
			
			std::vector<double> row;
			for(unsigned int j=0; j<chokeMatrix.size()+2; j++) {
				if(i<chokeMatrix.size() && j<chokeMatrix.size()) {
					row.push_back(chokeMatrix[i][j]);
				}
				else {
					row.push_back(0);
				}
			}
			splicedMatrix.push_back(row);
		}
		
		//Calculate the distances
		
		for(unsigned int i=0; i<splicedMatrix.size(); i++) {
			for(unsigned int j=0; j<splicedMatrix.size(); j++) {
				if(i == j) splicedMatrix[i][j] = 0;
				else {
					if(i > splicedMatrix.size() - 3 || j > splicedMatrix.size() - 3) {
						//Only calculate the start and end points,
						//the rest are already in the chokeMatrix.
						geometry_msgs::Point p_i;
						geometry_msgs::Point p_j;
						double bias_i = 1.0;
						double bias_j = 1.0;
						if(i < chokePoints.poses.size()) {
							p_i = chokePoints.poses[i].position;
							//Bias towards nav points
							if(tf::getYaw(chokePoints.poses[i].orientation) == 0) bias_i = 1.5;
						}
						else { 
							if(i == chokePoints.poses.size()) {
								p_i = from;
							}
							else {
								p_i = to;
							}
						}
						if(j < chokePoints.poses.size()) {
							p_j = chokePoints.poses[j].position;
							if(tf::getYaw(chokePoints.poses[j].orientation) == 0) bias_j = 1.5;
						}
						else { 
							if(j == chokePoints.poses.size()) {
								p_j = from;
							}
							else {
								p_j = to;
							}
						}
						if(splicedMatrix[i][j] == 0) {
							if(canGo(p_i, p_j)) {
								//canGo is asymmetric
								splicedMatrix[i][j] = distance(p_i, p_j) * bias_i * bias_j;
							}
							else {
								splicedMatrix[i][j] = 0;
							}
						}
					}
				}
				
			}
		}
		
		
		//A straight-up implementation of A* search on the matrix
		
		//Setup heuristics
		
		std::vector<double> heur;
		for(unsigned int j=0; j<splicedMatrix.back().size()-1; j++) {
			if(splicedMatrix.back().at(j) == 0 && j<chokePoints.poses.size()) {
				heur.push_back(distance(chokePoints.poses[j].position, to));
			}
			else { 
				if(splicedMatrix.back().at(j) != 0) {
					heur.push_back(splicedMatrix.back().at(j));
				}
				else {
					heur.push_back(distance(from, to));
				}
			}
		}
		heur.push_back(0);
		
		std::vector<bool> closed;
		std::vector<unsigned int> open;
		std::vector<int> came_from;
		nav_msgs::Path bestPath;
		std::vector<double> g_scores;
		std::vector<double> f_scores;
		
		for(unsigned int j=0; j<splicedMatrix.size(); j++) {
			closed.push_back(false);
			g_scores.push_back(DBL_MAX);
			f_scores.push_back(DBL_MAX);
			came_from.push_back(-1);
		}
		
		open.push_back(splicedMatrix.size()-2);
		g_scores[splicedMatrix.size()-2] = 0;
		f_scores[splicedMatrix.size()-2] = heur[splicedMatrix.size()-2];
		
		while(open.size() > 0) {
			
			unsigned int current = open[0];
			int currentIndex = 0;
			for(unsigned int i=0; i<open.size(); i++) {
				if(f_scores[open[i]] < f_scores[current]) {
					current = open[i];
					currentIndex = i;
				}
			}
			
			if(current == splicedMatrix.size()-1) { //Current is the goal
				
				std::vector<unsigned int> pathNodesReversed = reconstructPath(came_from, current);
				bestPath.poses.resize(pathNodesReversed.size());
				for(unsigned int x=pathNodesReversed.size()-1; pathNodesReversed.size() > 0; x--) {
					unsigned int nodeIndex = (unsigned int)pathNodesReversed.back();
					geometry_msgs::PoseStamped pathNode;
					if(nodeIndex == splicedMatrix.size()-1) {
						pathNode.pose.position = to;
						pathNode.pose.orientation.x = pathNode.pose.orientation.y = pathNode.pose.orientation.z = 0;
						pathNode.pose.orientation.w = 1;
					}
					else {
						if(nodeIndex == splicedMatrix.size()-2) {
							pathNode.pose.position = from;
							pathNode.pose.orientation.x = pathNode.pose.orientation.y = pathNode.pose.orientation.z = 0;
							pathNode.pose.orientation.w = 1;
						}
						else {
							pathNode.pose = chokePoints.poses[nodeIndex];
						}
					}
					
					pathNode.header.frame_id = "odom";
					pathNode.header.stamp = ros::Time::now();
					
					bestPath.poses[x]=(pathNode);
					pathNodesReversed.pop_back();
				}
				return bestPath;
			}
			
			open.erase(open.begin()+currentIndex);
			closed[current] = true;
			
			for(unsigned int i=0; i<splicedMatrix.size(); i++) {
				if(splicedMatrix.at(current).at(i) != 0) {
					if(!closed[i]) {
						double tmp_g = g_scores[current] + splicedMatrix[current][i];
						bool is_in_open = false;
						for(unsigned int j=0; j<open.size() && !is_in_open; j++) {
							is_in_open = (open[j] == i);
						}
						
						if(!is_in_open || tmp_g < g_scores[i]) {
							came_from[i] = current;
							g_scores[i] = tmp_g;
							f_scores[i] = tmp_g + heur[i];
							if(!is_in_open) {
								open.push_back(i);
							}
						}
					}
				}
			}
			
		}
		
		ROS_WARN("No path found to point.");
		
		//If no path is found, go to the nearest waypoint because the bot is probably stuck.
		
		unsigned int nearestWP = 0;
		double nearestWPDistance = DBL_MAX;
		
		for(unsigned int i=0; i<splicedMatrix.size(); i++) {
			if(i!=splicedMatrix.size()-2) {
				if(splicedMatrix[splicedMatrix.size()-2][i] < nearestWPDistance) {
					nearestWPDistance = splicedMatrix[splicedMatrix.size()-2][i];
					nearestWP = i;
				}
			}
		}
		
		geometry_msgs::PoseStamped currentPosition;
		geometry_msgs::PoseStamped destination;
		
		currentPosition.pose.position = from;
		currentPosition.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
		
		destination.pose = chokePoints.poses[nearestWP];
		
		bestPath.poses.resize(2);
		
		bestPath.poses[0] = currentPosition;
		bestPath.poses[1] = destination;
		
		return bestPath;
		
	}
	
	//ReconstructPath function of A* search
	std::vector<unsigned int> reconstructPath(std::vector<int> came_from, int currentNode) {
		if(came_from[currentNode] != -1) {
			std::vector<unsigned int> p = reconstructPath(came_from, came_from[currentNode]);
			p.push_back(currentNode);
			return p;
		}
		else {
			std::vector<unsigned int> p;
			p.push_back(currentNode);
			return p;
		}
	}
	
	void recordClearance(double clearance) {
		//Unused
		//std::cout << "Clearance : " << clearance << std::endl;
		tf::StampedTransform robotRelativeToTrajectory;      
		geometry_msgs::TransformStamped roboTransform;
		try {
			listener.lookupTransform("map", "base_link", ros::Time::now(), robotRelativeToTrajectory);
			tf::transformStampedTFToMsg(robotRelativeToTrajectory, roboTransform);
			geometry_msgs::Pose robotPose;
			robotPose.position.x = roboTransform.transform.translation.x;
			robotPose.position.y = roboTransform.transform.translation.y;
			robotPose.position.z = roboTransform.transform.translation.z;
			robotPose.orientation = roboTransform.transform.rotation;
			clearanceRecord cr;
			cr.clearance = clearance;
			cr.position = robotPose;
			clearances.push_back(cr);
			smoothClearances();
			while(clearances.size() > CLEARANCE_VECTOR_SIZE) {
				clearances.pop_front();
			}
		}
		catch(const tf::TransformException& ex) {
			ROS_ERROR("TF error %s,\r\n clearance not recorded.", ex.what());
		}
	}
	
	std::vector<std::vector<point2D> >* canGo(geometry_msgs::Point from, geometry_msgs::Point to) {
		
		//Using Bresenham's line algorithm and the midpoint circle algorithm,
		//it marks the cells a theoretical turtlebot would touch while 
		// moving from "from" to "to" (plus some safety margin).
		// If these cells are empty (occupancy < 25), it can go there.
		
		// Probably needs optimization, will be called frequently.
		
		/*
		
		Basically, the algorithm draws two circles, a smaller at the start
		and a larger at the end. Also, two lines.
		
		Then it fills the object, masks the map with the drawn shape
		and checks the map cells' values.
		
		(Assuming that the robot is at point "from", facing point "to").
		
		*/
		
		point2D c_from;
		point2D c_to;
		
		point2D l_from;
		point2D l_to;
		
		point2D r_from;
		point2D r_to;
		
		//Calculate the starting and ending points on the grid
		
		c_from.x = floor(from.x / map.info.resolution);
		c_from.y = floor(from.y / map.info.resolution);
		
		c_to.x = floor(to.x / map.info.resolution);
		c_to.y = floor(to.y / map.info.resolution);
		
		//Scan a straight line from the start to the end before doing anything else.
		
		double d = c_from.distance(c_to);
		double dx = (double)(c_to.x - c_from.x) / d;
		double dy = (double)(c_to.y - c_from.y) / d;
		
		{
			double curr_x = c_to.x;
			double curr_y = c_to.y;
			bool obstacle = false;
			while(!obstacle && floor(curr_x) != c_to.x && floor(curr_y) != c_to.y) {
				obstacle = (mapCell((int)floorf(curr_x), (int)floorf(curr_y)) >= 25) || (mapCell((int)floorf(curr_x), (int)floorf(curr_y)) < 0);
				curr_x += dx;
				curr_y += dy;
			}
			if(obstacle) return NULL;
		}
		
		double dst = distance(from, to);
		double dstMul = 1.0;
		if(dst / 2.0 < sqrt(MIN_RADIUS_MUL)) {
			dstMul = MIN_RADIUS_MUL;
		}
		else {
			if(dst / 2.0 < 1.0) dstMul = (dst * dst)/4.0;
			else dstMul = sqrt(dst/2.0);
		}
		if(dstMul > MAX_RADIUS_MUL) dstMul = MAX_RADIUS_MUL;
		
		//Calculate the bitmask size and offset
		
		point2D bitmaskMin;
		point2D bitmaskMax;
		
		if(c_from.x < c_to.x) {
			bitmaskMin.x = c_from.x;
			bitmaskMax.x = c_to.x;
		}
		else {
			bitmaskMin.x = c_to.x;
			bitmaskMax.x = c_from.x;
		}
		
		if(c_from.y < c_to.y) {
			bitmaskMin.y = c_from.y;
			bitmaskMax.y = c_to.y;
		}
		else {
			bitmaskMin.y = c_to.y;
			bitmaskMax.y = c_from.y;
		}
		
		bitmaskMin.x -= ceil(BOT_RADIUS / map.info.resolution) + 2;
		bitmaskMin.y -= ceil(BOT_RADIUS / map.info.resolution) + 2;
		bitmaskMax.x += ceil(BOT_RADIUS / map.info.resolution) + 2;
		bitmaskMax.y += ceil(BOT_RADIUS / map.info.resolution) + 2;
		
		BitMask mask (bitmaskMax.distanceX(bitmaskMin), bitmaskMax.distanceY(bitmaskMin));
		
		//Calculate the bearing from "from" to "to"
		
		double bearing = atan2(to.y - from.y, to.x - from.x);
		double leftAngle = bearing + M_PI*0.5;
		
		//Calculate the start and end of lines
		
		double x_diff = (cos(leftAngle) * BOT_RADIUS) * dstMul;
		double y_diff = (sin(leftAngle) * BOT_RADIUS) * dstMul;
		
		double x_diff_from = (cos(leftAngle) * BOT_RADIUS) * MIN_RADIUS_MUL;
		double y_diff_from = (sin(leftAngle) * BOT_RADIUS) * MIN_RADIUS_MUL;
		
		l_from.x = floor((from.x + x_diff_from) / map.info.resolution);
		l_from.y = floor((from.y + y_diff_from) / map.info.resolution);
		
		l_to.x = floor((to.x + x_diff) / map.info.resolution);
		l_to.y = floor((to.y + y_diff) / map.info.resolution);
		
		r_from.x = floor((from.x - x_diff_from) / map.info.resolution);
		r_from.y = floor((from.y - y_diff_from) / map.info.resolution);
		
		r_to.x = floor((to.x - x_diff) / map.info.resolution);
		r_to.y = floor((to.y - y_diff) / map.info.resolution);
		
		
		//Draw the lines and the circles
		
		std::vector<std::vector<point2D> > drawings;
		
		point2D l_from_mask = l_from - bitmaskMin;
		point2D l_to_mask = l_to - bitmaskMin;
		point2D r_from_mask = r_from - bitmaskMin;
		point2D r_to_mask = r_to - bitmaskMin;
		
		point2D c_from_mask = c_from - bitmaskMin;
		point2D c_to_mask = c_to - bitmaskMin;
		
		drawings.push_back(bresenham(l_from_mask, l_to_mask));
		
		drawings.push_back(bresenham(r_from_mask, r_to_mask));
		
		drawings.push_back(circle(c_from_mask, floor(BOT_RADIUS * MIN_RADIUS_MUL / map.info.resolution)));
		
		drawings.push_back(circle(c_to_mask, floor(BOT_RADIUS * dstMul / map.info.resolution)));
		
		for(unsigned int i=0; i<drawings.size(); i++) {
			mask.setPixels(drawings[i]);
		}
		
		//Fill the mask
		
		mask.fill();
		
		//Apply the mask on the map
		std::vector<char> masked = mask.applyMask(map, bitmaskMin);
		
		//Check if the predicted path contain obstacles
		
		bool navigable = true;
		
		for(unsigned int i=0; i<masked.size() && navigable; i++) {
			if(masked[i] > CENTER_THRESHOLD) {
				navigable = false;
			}
		}
		
		// If navigable, return the drawings. Might be useful for debugging.
		// If not, return a NULL.
		
		return navigable ? &drawings : NULL;
	}
	
	std::vector<point2D> bresenham(point2D from, point2D to) {
		// Bresenham's line algorithm
		
		std::vector<point2D> linePoints;
		
		double x1 = from.x;
		double y1 = from.y;
		double x2 = to.x;
		double y2 = to.y;
		
		const bool steep = (fabs(y2 - y1) > fabs(x2 - x1));
		if(steep)
		{
			std::swap(x1, y1);
			std::swap(x2, y2);
		}
		
		if(x1 > x2)
		{
			std::swap(x1, x2);
			std::swap(y1, y2);
		}
		
		const float dx = x2 - x1;
		const float dy = fabs(y2 - y1);
		
		float error = dx / 2.0f;
		const int ystep = (y1 < y2) ? 1 : -1;
		int y = (int)y1;
		
		const int maxX = (int)x2;
		
		for(int x=(int)x1; x<maxX; x++)
		{
			if(steep)
			{
				point2D p(y, x);
				linePoints.push_back(p);
			}
			else
			{
				point2D p(x, y);
				linePoints.push_back(p);
			}
			
			error -= dy;
			if(error < 0)
			{
				y += ystep;
				error += dx;
			}
		}
		
		return linePoints;
	}
	
	std::vector<point2D> circle(point2D o, unsigned int r) {
		//Midpoint circle algorithm
		
		int f = 1 - r;
		int ddF_x = 1;
		int ddF_y = -2 * r;
		int x = 0;
		int y = r;
		
		std::vector<point2D> output;
		
		output.push_back(point2D(o.x, o.y + r));
		output.push_back(point2D(o.x, o.y - r));
		output.push_back(point2D(o.x + r, o.y));
		output.push_back(point2D(o.x - r, o.y));
		
		while(x < y)
		{
			if(f >= 0) 
			{
				y--;
				ddF_y += 2;
				f += ddF_y;
			}
			x++;
			ddF_x += 2;
			f += ddF_x;    
			output.push_back(point2D(o.x + x, o.y + y));
			output.push_back(point2D(o.x - x, o.y + y));
			output.push_back(point2D(o.x + x, o.y - y));
			output.push_back(point2D(o.x - x, o.y - y));
			output.push_back(point2D(o.x + y, o.y + x));
			output.push_back(point2D(o.x - y, o.y + x));
			output.push_back(point2D(o.x + y, o.y - x));
			output.push_back(point2D(o.x - y, o.y - x));
		}
		
		return output;
	}
	
	int mapCell(int x, int y) {
		//Extract a map cell from the map
		//(y-1)*width+x, as it's in row-major order
		
		//1. transform the coordinates into the map's coordinate system
		
		int x_map = x - (map.info.origin.position.x / map.info.resolution);
		int y_map = y - (map.info.origin.position.y / map.info.resolution);
		
		//2. return the corresponding cell
		return map.data[y_map*map.info.width + x_map];
	}
	
	int mapCell(point2D p) { //Convenience method
		return mapCell(p.x, p.y);
	}
	
	enum FSM {FSM_MOVE_FORWARD, FSM_ROTATE};
	// Tunable parameters
	
	const static double MIN_SCAN_ANGLE_RAD = -75.0/180*M_PI;
	const static double MAX_SCAN_ANGLE_RAD = +75.0/180*M_PI;
	const static double CLEARANCE_LEFT_MIN = -0.75*M_PI;
	const static double CLEARANCE_LEFT_MAX = -0.25*M_PI;
	const static float PROXIMITY_RANGE_M = 0.45; // Should be smaller than sensor_msgs::LaserScan::range_max
	const static double MIN_OBSTACLE_RANGE = 0.3;
	const static double MAX_OBSTACLE_RANGE = 2.0;
	const static double FORWARD_SPEED_MPS = 0.6;
	const static double ROTATE_SPEED_RADPS = M_PI/2.0;
	
	const static unsigned int CLEARANCE_VECTOR_SIZE = 5;
	const static double CLEARANCE_MIN_DIFF = 0.15;
	const static double CLEARANCE_LASER_CUTOFF = 3.5;
	
	const static double CHOKEPOINT_MIN_DISTANCE = 0.2;
	
	const static double BOT_RADIUS = 0.3; //The robot radius, plus some space
	const static double MIN_RADIUS_MUL = 0.85;
	const static double MAX_RADIUS_MUL = 1.5;
	const static int LEFT_THRESHOLD = 50;
	const static int CENTER_THRESHOLD = 50;
	const static int RIGHT_THRESHOLD = 50; //Thresholds for navigation, in %
	
	
protected:
	ros::Publisher commandPub; // Publisher to the simulated robot's velocity command topic
	ros::Subscriber laserSub; // Subscriber to the simulated robot's laser scan topic
	ros::Subscriber mapSub; //Subscriber to the occupancy grid (for navigation)
	ros::Subscriber targetSub; //Subscriber to the nav target
	ros::Subscriber initialPoseSub; //Subscriber for initial pose
	ros::Subscriber tfSub; //Subscribes to transformation messages so it could determine the robot's location
	ros::Publisher trajectoryPub; //Publishes the planned trajectory to Rviz
	enum FSM fsm; // Finite state machine for the random walk algorithm
	ros::Time rotateStartTime; // Start time of the rotation
	ros::Duration rotateDuration; // Duration of the rotation
	tf::TransformListener listener;
	nav_msgs::OccupancyGrid map;
    bool mapCached;
	ros::ServiceClient mapClient;
	ros::ServiceClient frontierClient;
	geometry_msgs::PoseStamped goal;     
	nav_msgs::Path trajectory;
	bool firstTimePlanning;
	tf::StampedTransform trajectoryTransform;
	
	//For recording and publishing choke points
	std::deque<clearanceRecord> clearances;
	std::deque<clearanceRecord> smoothed;
	geometry_msgs::PoseArray chokePoints;
	ros::Publisher chokePointPub;
	
	std::vector<std::vector<double> > chokeMatrix;
	
	nav_msgs::OccupancyGrid chokeGraph;
	ros::Publisher chokeGraphPub;
	
	double currentSpeed;
	double currentSensorRange;
	double lastScanMinRange;



public:
    ros::ServiceServer vantageDistanceServer;
};

int main(int argc, char **argv) {
	ros::init(argc, argv, "turtle_nav"); // Initiate new ROS node named "turtle_nav"
	ros::NodeHandle n;
    TurtleNav navigator(n); // Create new TurtleNav object
    navigator.vantageDistanceServer = n.advertiseService("get_distances", &TurtleNav::getDistances, &navigator);
    navigator.spin(); // Execute FSM loop
	return 0;
};

